import sys
from PyQt5.QtWidgets import QApplication, QMainWindow, QDialog
import threading
import time
import os

sys.path.append(os.getcwd()+'/src/cranebot/cranebot')

from joint_client import *
from qt_window import Ui_MainWindow

class MyWindow(QMainWindow, Ui_MainWindow):
    def __init__(self, parent=None):
        super( MyWindow, self).__init__(parent)
        self.setupUi(self)
        self.apply.clicked.connect(self.apply_action)
        self.cancel.clicked.connect(self.cancel_action)
        self.client = Client()
        self.show_current_position()

    def show_current_position(self):
        try:
            raise_position = round(self.client.transform_joint_distance("raise"), 2)
            extend_position = round(self.client.transform_joint_distance("extend"), 2)
            rotate_position = round(self.client.transform_joint_distance("rotate"), 2)
            lift_position = round(self.client.transform_joint_distance("lift"), 2)
            self.label_13.setText(str(raise_position))
            self.label_14.setText(str(extend_position))
            self.label_15.setText(str(rotate_position))
            self.label_16.setText(str(lift_position))
        except:
            pass

    def apply_action(self):
        raise_goal = self.doubleSpinBox_raise.value()
        extend_goal = self.doubleSpinBox_extend.value()
        rotate_goal = self.doubleSpinBox_rotate.value()
        lift_goal = self.doubleSpinBox_lift.value()
        self.show_current_position()
        if self.checkBox.isChecked():
            t1 = threading.Thread(target = self.client.operation_by_joint, args=(raise_goal, extend_goal, rotate_goal, lift_goal, False))
            t1.start()
            #self.client.operation_no_hup(raise_goal, extend_goal, rotate_goal, lift_goal, False)
        else:
            t1 = threading.Thread(target = self.client.operation_by_joint, args=(raise_goal, extend_goal, rotate_goal, lift_goal))
            t1.start()

    def cancel_action(self):
        self.client.cancel_goal()
        self.show_current_position()

if __name__ == '__main__':
    if not rclpy.ok():
        rclpy.init()
    app = QApplication(sys.argv)
    MainWindow = MyWindow()
    MainWindow.show()
    sys.exit(app.exec_())
    
